{
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "language": "python",
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "nbconvert_exporter": "python",
      "version": "3.5.2",
      "pygments_lexer": "ipython3",
      "name": "python",
      "file_extension": ".py",
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "mimetype": "text/x-python"
    }
  },
  "nbformat": 4,
  "cells": [
    {
      "source": [
        "%matplotlib inline"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "\n********************************************************************************\n2D Robot SLAM - Example\n********************************************************************************\nGoals of this script:\n\n- apply the UKF for performing 2D SLAM (Simultaneous Localization And Mapping).\n\n- discover a computationally alternative way for performing UKF inspired from\n  :cite:`HuangA2013`. This alternative leads to computational speed improvement\n  when only a part of the state is involved in a propagation or in update step.\n\n- augment the state when a new landmark is observed in a UKF derivative-free\n  way.\n\n*We assume the reader is already familiar with the approach described in the\ntutorial.*\n\nThis script considers the 2D robot SLAM problem where the robot is equipped with\nwheel odometry and observes unknown landmark measurements. The robot state is\npropagated through the odometry model and landmark observations are used in the\nUKF measurement step. Landmarks are static and we assume no error coming from\ndata association. We reproduce the simulations that are described in\n:cite:`huangObservability2010` , :cite:`HuangA2013`.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Import\n==============================================================================\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "import ukfm\nfrom ukfm import SLAM2D as MODEL\nimport numpy as np\nimport matplotlib\nukfm.set_matplotlib_config()"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "Model and Simulation\n==============================================================================\nThis script uses the :meth:`~ukfm.SLAM2D` model that requires sequence time\nand odometry frequency.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "# sequence time (s)\nT = 2500\n# odometry frequency (Hz)\nodo_freq = 1\n# create the model\nmodel = MODEL(T, odo_freq)"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "The trajectory of the robot consists in turning at constant speed.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "# true speed of robot (m/s)\nv = 0.25\n# true angular velocity (rad/s)\ngyro = 1.5 / 180 * np.pi\n# odometry noise standard deviation\nodo_std = np.array([0.05*v/np.sqrt(2),     # speed (v/m)\n                    0.05*v*np.sqrt(2)*2])  # angular speed (rad/s)"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "When simulating data, we generate a map. The map is defined as landmarks\nconstantly spaced on a circle with slightly higher radius than the radius of\nthe robot trajectory.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "# simulate true trajectory and noisy input\nstates, omegas, ldks = model.simu_f(odo_std, v, gyro)\n# observation noise standard deviation (m)\nobs_std = 0.1\n# plot the map\nmodel.plot_traj(states, ldks)"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "The state and the input contain the following variables:\n\n.. highlight:: python\n.. code-block:: python\n\n    states[n].Rot      # orientation (matrix)\n    states[n].p        # robot position\n    states[n].p_l      # landmark positions\n    omegas[n].gyro     # robot angular velocity\n    omegas[n].v        # robot speed\n\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Landmark positions are a 2D array where we get the k-th landmark as\n``states[n].p_l[k]``. The number of landmarks in the state starts from zero\nand increases when the robot observes a new landmark.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "We compute noisy landmark measurements based on the true states.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "ys = model.simu_h(states, obs_std, ldks)"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "A measurement contains the observations of all visible landmarks as:\n\n.. highlight:: python\n.. code-block:: python\n\n   y_n = ys[n]    # measurement at timestamp n\n   y_n_k = y_n[k] # k-th observed landmark at instant n, where y_n_k[2] is the\n                  # landmark index (-1 if the landmark is not observed)\n\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Filter Design and Initialization\n------------------------------------------------------------------------------\nWe embed the robot state in $SO(2) \\times \\mathbb{R}^2$ and each\nlandmark position in $\\mathbb{R}^2$, such that:\n\n- the retraction $\\varphi(.,.)$ is the $SO(2)$ exponential for\n  orientation, and the vector addition for positions.\n\n- the inverse retraction $\\varphi^{-1}_.(.)$ is the $SO(2)$\n  logarithm for orientation and the vector subtraction for positions.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Remaining parameter setting is standard. \n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "# propagation noise covariance matrix\nQ = np.diag(odo_std**2)\n# measurement noise covariance matrix\nR = obs_std**2*np.eye(2)\n# sigma point parameters\nalpha = np.array([1e-3, 1e-3, 1e-3, 1e-3, 1e-3])\n# initial uncertainty matrix\nP0 = np.zeros((3, 3))  # The state is perfectly initialized without"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "Regarding implementation, we use the Jacobian UKF (:meth:`~ukfm.JUKF`) that\nspares time when only a part of the space is involved in a propagation or\nupdate step.\n\n**How it works ?** Consider the propagation of the covariance in an extended\nKalman filter as\n\n\\begin{align}\\mathbf{P}_{n+1} = \\mathbf{F} \\mathbf{P}_{n} \\mathbf{F}^T +\n  \\mathbf{G} \\mathbf{Q} \\mathbf{G}^T,\\end{align}\n\nwhere the robot state uncertainty is put in the first indices of the\ncovariance matrix $\\mathbf{P}_{n}$. As landmarks are statics, the\nJacobian take the forms\n\n\\begin{align}\\mathbf{F} = \\begin{bmatrix}  \\mathbf{F}^R & \\mathbf{0} \\\\\n  \\mathbf{0}  & \\mathbf{I}  \\end{bmatrix}, \\mathbf{G} = \\begin{bmatrix}\n  \\mathbf{G}^R & \\mathbf{0} \\\\ \\mathbf{0}  & \\mathbf{0}  \\end{bmatrix}.\\end{align}\n\nThe JUKF allows to compute $\\mathbf{F}^R$ and $\\mathbf{G}^R$ by\nonly using the required sigma points. Here it corresponds to the sigma points\nof the robot state. This requires to set the reduced retraction ``red_phi``\nand inverse retraction ``red_phi_inv`` that compute the required  subpart of\nthe full retraction $\\varphi(.,.)$ and inverse retraction\n$\\varphi^{-1}_.(.)$, and to define corresponding indices ``red_idx`` in\n$\\mathbf{P}_n$.\n\nSimilarly for the observation of a landmark, e.g. the first landmark, the\nobservation matrix has the form\n\n\\begin{align}\\mathbf{H} = \\begin{bmatrix}  \\mathbf{H}^1 & \\mathbf{0} \\end{bmatrix}.\\end{align}\n\nThe JUKF computes $\\mathbf{H}^1$ by only using the required sigma points\nof the robot state and the observed landmark. This requires to set another\nfunction ``up_phi`` using during update to compute a subpart of the retraction\n$\\varphi(.,.)$, as corresponding indices ``up_idx`` in\n$\\mathbf{P}_n$.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Finally, we require to define a new function $z(.,.)$ to augment the\nstate such that\n\n\\begin{align}\\boldsymbol{\\chi}_n^{\\mathrm{aug}} = z(\\boldsymbol{\\chi}_{n}, \\mathbf{y}_n),\\end{align}\n\nwhere $\\boldsymbol{\\chi}_n^{\\mathrm{aug}}$ is the augmented state and\nthe $\\mathbf{y}_n$ the measurement used to augment the state. Here this\nmeasurement is a landmark observation. To make the augmentation efficient we\nneed to compute sigma points for only the state involved in $z(.,.)$,\n``aug_phi`` is thus only a subpart of $\\varphi(.,.)$ and ``aug_inv_phi``\nis the inverse retraction of $\\varphi(.,.)^{-1}$ corresponding to the\nnovel part of the state only.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "# reduced weights during propagation\nred_idxs = np.array([0, 1, 2])  # indices corresponding to the robot state in P\n# weights during update\naug_idxs = np.array([0, 1, 2])  # indices corresponding to the robot state in P\n\nstate0 = model.STATE(Rot=states[0].Rot, p=states[0].p, p_l=np.zeros((0, 2)))\n\nukf = ukfm.JUKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, phi=model.phi,\n                alpha=alpha, \n                red_phi=model.red_phi,\n                red_phi_inv=model.red_phi_inv,\n                red_idxs=red_idxs,\n                up_phi=model.up_phi,\n                up_idxs=np.arange(5),  # it will changes during the sequence\n                aug_z=model.aug_z,\n                aug_phi=model.aug_phi,\n                aug_phi_inv=model.aug_phi_inv,\n                aug_idxs=aug_idxs,\n                aug_q=2)\n\n# set variables for recording estimates along the full trajectory\nukf_states = [states[0]]\nukf_Ps = [P0]\n\n# indices of already observed landmarks\nukf_lmk = np.array([])"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "Filtering\n==============================================================================\nThe UKF proceeds as a standard Kalman filter with a for loop.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "for n in range(1, model.N):\n    # propagation\n    ukf.propagation(omegas[n-1], model.dt)\n    y_n = ys[n]\n    # observed landmarks\n    idxs = np.where(y_n[:, 2] >= 0)[0]\n    # update each landmark already in the filter\n    p_ls = ukf.state.p_l\n    for idx0 in idxs:\n        idx = np.where(ukf_lmk == y_n[idx0, 2])[0]\n        if idx.shape[0] == 0:\n            continue\n        # indices of the robot and observed landmark in P\n        up_idxs = np.hstack([0, 1, 2, 3+2*idx, 4+2*idx])\n        ukf.state.p_l = np.squeeze(p_ls[idx])\n        # compute observability matrices and residual\n        ukf.H_num(np.squeeze(y_n[idx0, :2]), up_idxs, R)\n    ukf.state.p_l = p_ls\n    # update only if some landmarks have been observed\n    if ukf.H.shape[0] > 0:\n        ukf.state_update()\n    # augment the state with new landmark\n    for idx0 in idxs:\n        idx = np.where(ukf_lmk == y_n[idx0, 2])[0]\n        if not idx.shape[0] == 0:\n            continue\n        # augment the landmark state\n        ukf_lmk = np.hstack([ukf_lmk, int(y_n[idx0, 2])])\n        # indices of the new landmark\n        idx = ukf_lmk.shape[0]-1\n        # new landmark position\n        p_l = np.expand_dims(ukf.state.p + ukf.state.Rot.dot(y_n[idx0, :2]), 0)\n        p_ls = np.vstack([ukf.state.p_l, p_l])\n        ukf.state.p_l = p_l\n        # get Jacobian and then covariance following [2]\n        R_n = obs_std ** 2 * np.eye(2)\n        ukf.aug(y_n[idx0, :2], aug_idxs, R)\n        ukf.state.p_l = p_ls\n    # save estimates\n    ukf_states.append(ukf.state)\n    ukf_Ps.append(ukf.P)"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "Results\n------------------------------------------------------------------------------\nWe plot the trajectory, the position of the landmarks and the estimated\ntrajectory in the same plot, the attitude error, the position error, and their\nconfidence interval.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "source": [
        "model.plot_results(ukf_states, ukf_Ps, states, ldks)"
      ],
      "metadata": {
        "collapsed": false
      },
      "execution_count": null,
      "cell_type": "code",
      "outputs": []
    },
    {
      "metadata": {},
      "source": [
        "We note the $3\\sigma$ confidence interval decreases along time.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Conclusion\n==============================================================================\nThis script shows how the UKF on parallelizable manifolds can be used for 2D\nSLAM. By leveraging numerical Jacobian inference, one obtains a\ncomputationally more efficient filter. The UKF works for this example, but\nconsistency issues happear at the end of the trajectory.\n\nYou can now:\n\n- consider non-linear range and bearing measurement.\n\n- benchmark the UKF with different retractions and compare it to the extended\n  Kalman filter and the invariant extended Kalman filter of\n  :cite:`barrauInvariant2017`.\n\n"
      ],
      "cell_type": "markdown"
    }
  ]
}